using System;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework
{
	/// <summary>
	/// Represents a relative direction of a <see cref="Microsoft.Xna.Framework.Quaternion" />.
	/// </summary>
	public enum QuaternionAxis
	{
		/// <summary>
		/// The direction pointing right from a <see cref="Microsoft.Xna.Framework.Quaternion" />.
		/// </summary>
		Right,

		/// <summary>
		/// The direction pointing up from a <see cref="Microsoft.Xna.Framework.Quaternion" />.
		/// </summary>
		Up,

		/// <summary>
		/// The direction pointing forward from a <see cref="Microsoft.Xna.Framework.Quaternion" />.
		/// </summary>
		Forward
	}

	/// <summary>
	/// An extension to the <see cref="Microsoft.Xna.Framework.Quaternion" /> structure. 
	/// </summary>
	public static class QuaternionExtension
	{
		/// <summary>
		/// 
		/// </summary>
		/// <param name="source"></param>
		/// <param name="destination"></param>
		/// <returns></returns>
		public static Quaternion CreateRotationTo(Vector3 source, Vector3 destination)
		{
			Quaternion result = new Quaternion();
		    
			float d = Vector3Extension.Dot(ref source, ref destination);

            // If dot == 1, vectors are the same
			if (d >= 1.0f)
			{
				return Quaternion.Identity;
			}

			if (d < (1e-6f - 1.0f))
			{
				// Generate an axis
				Vector3 axis = Vector3.Cross(Vector3.UnitX, source);
				if (Vector3Extension.IsZeroLength(ref axis)) // pick another if colinear
				{
					axis = Vector3.Cross(Vector3.UnitX, source);
				}
				axis.Normalize();

				result = Quaternion.CreateFromAxisAngle(axis, MathHelper.Pi);
			}
			else
			{
                float s = (float)Math.Sqrt((1 + d) * 2.0f);
	            float invs = 1.0f / s;

				Vector3 c = Vector3.Cross(source, destination);
				
    	        result.X = c.X * invs;
        	    result.Y = c.Y * invs;
            	result.Z = c.Z * invs;
            	result.W = s * 0.5f;
				result.Normalize();
			}

            return result;
		}

		/// <summary>
		/// Extracts the forward, right, and up vectors from a <see cref="Microsoft.Xna.Framework.Quaternion"/>. 
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <param name="forward">Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing the forward rotational axis.</param>
		/// <param name="right">Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing the right rotational axis.</param>
		/// <param name="up">Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing the up rotational axis.</param>
		public static void Decompose(ref Quaternion quaternion, out Vector3 forward, out Vector3 right, out Vector3 up)
		{
			float xx = quaternion.X * quaternion.X;
			float yy = quaternion.Y * quaternion.Y;
			float zz = quaternion.Z * quaternion.Z;
			float xy = quaternion.X * quaternion.Y;
			float zw = quaternion.Z * quaternion.W;
			float zx = quaternion.Z * quaternion.X;
			float yw = quaternion.Y * quaternion.W;
			float yz = quaternion.Y * quaternion.Z;
			float xw = quaternion.X * quaternion.W;

#if XBOX 
			forward = new Vector3(2.0f * (zx + yw), 2.0f * (yz - xw), 1.0f - 2.0f * (yy + xx));
#else
			forward.X = 2.0f * (zx + yw);
			forward.Y = 2.0f * (yz - xw);
			forward.Z = 1.0f - 2.0f * (yy + xx);
#endif

#if XBOX 
			right = new Vector3(1.0f - 2.0f * (yy + zz), 2.0f * (xy + zw), 2.0f * (zx - yw));
#else
			right.X = 1.0f - 2.0f * (yy + zz);
			right.Y = 2.0f * (xy + zw);
			right.Z = 2.0f * (zx - yw);
#endif

#if XBOX
			up = new Vector3(2.0f * (xy - zw), 1.0f - 2.0f * (zz + xx), 2.0f * (yz + xw));
#else
			up.X = 2.0f * (xy - zw);
			up.Y = 1.0f - 2.0f * (zz + xx);
			up.Z = 2.0f * (yz + xw);
#endif
		}

		/// <summary>
		/// Gets the forward rotational axis from the specified <see cref="Microsoft.Xna.Framework.Quaternion"/>.
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <returns>Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing the forward rotational axis.</returns>
		public static Vector3 GetForwardAxis(Quaternion quaternion)
		{
#if XBOX
			Vector3 result = new Vector3();
#else
			Vector3 result;
#endif
			result.X = 2.0f * (quaternion.Z * quaternion.X + quaternion.Y * quaternion.W);
			result.Y = 2.0f * (quaternion.Y * quaternion.Z - quaternion.X * quaternion.W);
			result.Z = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.X * quaternion.X);

			return result;
		}

		/// <summary>
		/// Gets the forward rotational axis from the specified <see cref="Microsoft.Xna.Framework.Quaternion"/>. 
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <param name="result">Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing the forward rotational axis.</param>
		public static void GetForwardAxis(ref Quaternion quaternion, out Vector3 result)
		{
#if XBOX
			result = new Vector3();
#endif
			result.X = 2.0f * (quaternion.Z * quaternion.X + quaternion.Y * quaternion.W);
			result.Y = 2.0f * (quaternion.Y * quaternion.Z - quaternion.X * quaternion.W);
			result.Z = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.X * quaternion.X);
		}

		/// <summary>
		/// Gets the right rotational axis from the specified <see cref="Microsoft.Xna.Framework.Quaternion"/>.
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <returns>Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing the right rotational axis.</returns>
		public static Vector3 GetRightAxis(Quaternion quaternion)
		{
#if XBOX
			Vector3 result = new Vector3();
#else
			Vector3 result;
#endif
			result.X = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
			result.Y = 2.0f * (quaternion.X * quaternion.Y + quaternion.Z * quaternion.W);
			result.Z = 2.0f * (quaternion.Z * quaternion.X - quaternion.Y * quaternion.W);

			return result;
		}

		/// <summary>
		/// Gets the right rotational axis from the specified <see cref="Microsoft.Xna.Framework.Quaternion"/>.
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <param name="result">Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing the right rotational axis.</param>
		public static void GetRightAxis(ref Quaternion quaternion, out Vector3 result)
		{
#if XBOX
			result = new Vector3();
#endif
			result.X = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
			result.Y = 2.0f * (quaternion.X * quaternion.Y + quaternion.Z * quaternion.W);
			result.Z = 2.0f * (quaternion.Z * quaternion.X - quaternion.Y * quaternion.W);
		}

		/// <summary>
		/// Gets the up rotational axis from the specified <see cref="Microsoft.Xna.Framework.Quaternion"/>.
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <returns>Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing the up rotational axis.</returns>
		public static Vector3 GetUpAxis(Quaternion quaternion)
		{
#if XBOX
			Vector3 result = new Vector3();
#else
			Vector3 result;
#endif
			result.X = 2.0f * (quaternion.X * quaternion.Y - quaternion.Z * quaternion.W);
			result.Y = 1.0f - 2.0f * (quaternion.Z * quaternion.Z + quaternion.X * quaternion.X);
			result.Z = 2.0f * (quaternion.Y * quaternion.Z + quaternion.X * quaternion.W);

			return result;
		}

		/// <summary>
		/// Gets the up rotational axis from the specified <see cref="Microsoft.Xna.Framework.Quaternion"/>.
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <param name="result">Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing the up rotational axis.</param>
		public static void GetUpAxis(ref Quaternion quaternion, out Vector3 result)
		{
#if XBOX
			result = new Vector3();
#endif
			result.X = 2.0f * (quaternion.X * quaternion.Y - quaternion.Z * quaternion.W);
			result.Y = 1.0f - 2.0f * (quaternion.Z * quaternion.Z + quaternion.X * quaternion.X);
			result.Z = 2.0f * (quaternion.Y * quaternion.Z + quaternion.X * quaternion.W);
		}

		/// <summary>
		/// Gets the chosen rotational axis from the specified <see cref="Microsoft.Xna.Framework.Quaternion"/>.
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <param name="axis">The rotational axis to extract.</param>
		/// <returns>Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing a rotational axis.</returns>
		public static Vector3 GetAxis(this Quaternion quaternion, QuaternionAxis axis)
		{
#if XBOX
			Vector3 result = new Vector3();
#else
			Vector3 result;
#endif
			switch (axis)
			{
				case QuaternionAxis.Forward:
					result.X = 2.0f * (quaternion.Z * quaternion.X + quaternion.Y * quaternion.W);
					result.Y = 2.0f * (quaternion.Y * quaternion.Z - quaternion.X * quaternion.W);
					result.Z = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.X * quaternion.X);

					return result;

				case QuaternionAxis.Right:
					result.X = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
					result.Y = 2.0f * (quaternion.X * quaternion.Y + quaternion.Z * quaternion.W);
					result.Z = 2.0f * (quaternion.Z * quaternion.X - quaternion.Y * quaternion.W);

					return result;

				case QuaternionAxis.Up:
					result.X = 2.0f * (quaternion.X * quaternion.Y - quaternion.Z * quaternion.W);
					result.Y = 1.0f - 2.0f * (quaternion.Z * quaternion.Z + quaternion.X * quaternion.X);
					result.Z = 2.0f * (quaternion.Y * quaternion.Z + quaternion.X * quaternion.W);

					return result;

				default:
					throw new ArgumentException();
			}
		}

		/// <summary>
		/// Gets the chosen rotational axis from the specified <see cref="Microsoft.Xna.Framework.Quaternion"/>.
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <param name="axis">The rotational axis to extract.</param>
		/// <returns>Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing a rotational axis.</returns>
		public static Vector3 GetAxis(ref Quaternion quaternion, QuaternionAxis axis)
		{
#if XBOX
			Vector3 result = new Vector3();
#else
			Vector3 result;
#endif
			switch (axis)
			{
				case QuaternionAxis.Forward:
					result.X = 2.0f * (quaternion.Z * quaternion.X + quaternion.Y * quaternion.W);
					result.Y = 2.0f * (quaternion.Y * quaternion.Z - quaternion.X * quaternion.W);
					result.Z = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.X * quaternion.X);

					return result;

				case QuaternionAxis.Right:
					result.X = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
					result.Y = 2.0f * (quaternion.X * quaternion.Y + quaternion.Z * quaternion.W);
					result.Z = 2.0f * (quaternion.Z * quaternion.X - quaternion.Y * quaternion.W);

					return result;

				case QuaternionAxis.Up:
					result.X = 2.0f * (quaternion.X * quaternion.Y - quaternion.Z * quaternion.W);
					result.Y = 1.0f - 2.0f * (quaternion.Z * quaternion.Z + quaternion.X * quaternion.X);
					result.Z = 2.0f * (quaternion.Y * quaternion.Z + quaternion.X * quaternion.W);

					return result;

				default:
					throw new ArgumentException();
			}
		}

		/// <summary>
		/// Gets the chosen rotational axis from the specified <see cref="Microsoft.Xna.Framework.Quaternion"/>.
		/// </summary>
		/// <param name="quaternion">The source <see cref="Microsoft.Xna.Framework.Quaternion"/>.</param>
		/// <param name="axis">The rotational axis to extract.</param>
		/// <param name="result">Returns a <see cref="Microsoft.Xna.Framework.Vector3"/> representing a rotational axis.</param>
		public static void GetAxis(ref Quaternion quaternion, QuaternionAxis axis, out Vector3 result)
		{
#if XBOX
			result = new Vector3();
#endif
			switch (axis)
			{
				case QuaternionAxis.Forward:
					result.X = 2.0f * (quaternion.Z * quaternion.X + quaternion.Y * quaternion.W);
					result.Y = 2.0f * (quaternion.Y * quaternion.Z - quaternion.X * quaternion.W);
					result.Z = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.X * quaternion.X);

					break;

				case QuaternionAxis.Right:
					result.X = 1.0f - 2.0f * (quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z);
					result.Y = 2.0f * (quaternion.X * quaternion.Y + quaternion.Z * quaternion.W);
					result.Z = 2.0f * (quaternion.Z * quaternion.X - quaternion.Y * quaternion.W);

					break;

				case QuaternionAxis.Up:
					result.X = 2.0f * (quaternion.X * quaternion.Y - quaternion.Z * quaternion.W);
					result.Y = 1.0f - 2.0f * (quaternion.Z * quaternion.Z + quaternion.X * quaternion.X);
					result.Z = 2.0f * (quaternion.Y * quaternion.Z + quaternion.X * quaternion.W);

					break;

				default:
					throw new ArgumentException();
			}
		}
	}
}
